#!/usr/bin/env python3
# -*- encoding: utf-8 -*-
"""
@File         :switch_fw_mode.py
@Description  :
@Time         :2024/05/29 22:14:36
@Author       :Lin Yuheng
@Version      :1.0
"""

import rospy, time
from mavros_msgs.msg import State
from mavros_msgs.srv import SetMode, CommandBool, SetModeRequest, SetModeResponse
from switch_fw_mode import SwitchFWMode


class FormationSwitchFWMode:
    def __init__(self):
        
        self.mode_request = SetModeRequest()
        self.outflag = False  # 退出标志位
        self.counters = 0  # 计数器
        self.times_out = 20
        
        self.switch_fw1_mode = SwitchFWMode()
        self.switch_fw2_mode = SwitchFWMode()
        self.switch_fw3_mode = SwitchFWMode()
        
        self.switch_fw1_mode.set_planeID(1)
        self.switch_fw2_mode.set_planeID(2)
        self.switch_fw3_mode.set_planeID(3)
        
    def arm(self):
        self.switch_fw1_mode.arm_service = self.switch_fw1_mode.arm()
        time.sleep(0.5)
        self.switch_fw2_mode.arm_service = self.switch_fw2_mode.arm()
        time.sleep(0.5)
        self.switch_fw3_mode.arm_service = self.switch_fw3_mode.arm()
        
        # if self.switch_fw1_mode.arm_service(True) and self.switch_fw2_mode.arm_service(True) and self.switch_fw3_mode.arm_service(True):
        #     rospy.loginfo("一键解锁成功")
        # else:
        #     rospy.loginfo("一键解锁失败")
        
    def get_ros_time(self, begin: rospy.Time):
        time_now = rospy.Time.now()
        currTimeSec = time_now.secs - begin.secs
        currTimenSec = time_now.nsecs / 1e9 - begin.nsecs / 1e9
        return currTimeSec + currTimenSec
        
    def start(self):
        begin_time = rospy.Time.now()
        current_time = self.get_ros_time(begin_time)
        rate = rospy.Rate(5)
        
        self.switch_fw1_mode.ros_sub_pub()
        self.switch_fw2_mode.ros_sub_pub()
        self.switch_fw3_mode.ros_sub_pub()
        
        while not rospy.is_shutdown():
            print(
                ">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>FORMATION_SWITCH_MODE<<<<<<<<<<<<<<<<<<<<<<<<<<"
            )
            rospy.loginfo("当前时刻:%f", current_time)
            rospy.loginfo("切换模式: 1 一键解锁, 2 一键起飞, 3 编队, 4 loiter, 5 out")
            # 获取键盘指令
            
            try:
                cmd = input("请输入指令:")

                if cmd == "1":
                    self.arm()
                elif cmd == "2":
                    self.mode_request.custom_mode = "AUTO.TAKEOFF"
                elif cmd == "3":
                    self.mode_request.custom_mode = "OFFBOARD"
                elif cmd == "4":
                    self.mode_request.custom_mode = "AUTO.LOITER"
                elif cmd == "5":
                    self.outflag = True
                    break
                else:
                    print("无效指令，重新输入")

                while (
                    cmd in ['2','3','4']
                    # and (self.mode_request.custom_mode != self.current_state.mode)
                    and (self.counters <= self.times_out)
                ):
                    self.counters += 1
                    self.switch_fw1_mode.set_mode_service.call(self.mode_request)
                    self.switch_fw2_mode.set_mode_service.call(self.mode_request)
                    self.switch_fw3_mode.set_mode_service.call(self.mode_request)

                self.counters = 0
                rate.sleep()

            except rospy.ROSException as e:
                pass

        rospy.loginfo("switch_fw_mode end")

if __name__ == "__main__":
    rospy.init_node("formation_switch_fw_node")
    formation_switch_fw_mode = FormationSwitchFWMode()
    formation_switch_fw_mode.start()

